#include "stm32f10x.h" 
#include "config.h" //引入全局常量配置信息
#include "Motor.h"

// 条件编译
#if(isWork_OLED == 1)
    #include "OLED.h"
#endif

// 声明是在外部定义的变量
extern uint8_t irState[4];
extern int mainSpeed;
extern int spinSpeed; // 原地转反向速度
extern int backSpeed; // 后退速度
extern int count;

// 定义红外循迹模块的引脚
#define IR_SENSOR_PORT GPIOA // 请根据实际引脚分配进行修改_PORT GPIOA 

void IR_Init(void) {
    // 启用GPIOA时钟
    RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE);
	
    // 配置红外循迹模块的GPIO引脚
    GPIO_InitTypeDef GPIO_InitStruct;
    GPIO_InitStruct.GPIO_Pin = GPIO_Pin_10 | GPIO_Pin_11 | GPIO_Pin_9 | GPIO_Pin_12;
    GPIO_InitStruct.GPIO_Mode = GPIO_Mode_IPU; // 使用上拉输入模式
    GPIO_InitStruct.GPIO_Speed = GPIO_Speed_50MHz;
    GPIO_Init(IR_SENSOR_PORT, &GPIO_InitStruct);
}

// 读取四个红外循迹模块的状态，并将结果存储在irState数组中
void IR_ReadState(void) {
    // 0表示无信号，1表示有信号
    irState[0] = GPIO_ReadInputDataBit(IR_SENSOR_PORT, GPIO_Pin_9) == Bit_SET ? 0 : 1;
    irState[1] = GPIO_ReadInputDataBit(IR_SENSOR_PORT, GPIO_Pin_10) == Bit_SET ? 0 : 1;
    irState[2] = GPIO_ReadInputDataBit(IR_SENSOR_PORT, GPIO_Pin_11) == Bit_SET ? 0 : 1;
    irState[3] = GPIO_ReadInputDataBit(IR_SENSOR_PORT, GPIO_Pin_12) == Bit_SET ? 0 : 1;
}

void track(int speedLeft,int speedRight){
    IR_ReadState(); // 读取循迹传感器数据
    #if(isWork_OLED == 1)
        OLED_ShowNum(1,1,irState[0],1);
        OLED_ShowNum(1,3,irState[1],1);
        OLED_ShowNum(1,5,irState[2],1);
        OLED_ShowNum(1,7,irState[3],1);
    #endif
    if (irState[0] && !irState[1] && !irState[2] && irState[3])
    {// 1001
        // 前进
        Motor_forward(speedLeft,speedRight);
        #if(isWork_OLED == 1)
            OLED_ShowString(1, 9, "F");
        #endif
    }else if (irState[0] && !irState[1] && irState[2] && irState[3])
    {//1011
        // 小左转
        Motor_diffLeft(speedLeft);
        #if(isWork_OLED == 1)
            OLED_ShowString(1, 9, "sL");
        #endif
    }else if (irState[0] && irState[1] && !irState[2] && irState[3])
    {//1101
        // 小右转
        Motor_diffRight(speedLeft);
        #if(isWork_OLED == 1)
            OLED_ShowString(1, 9, "sR");
        #endif
    }else if ((!irState[0] && irState[1] && irState[2] && irState[3])
                ||(!irState[0] && !irState[1] && irState[2] && irState[3])
                ||(!irState[0] && !irState[1] && !irState[2] && irState[3]))
    {//0111 or 0011 or 0001
        // 大左转
        Motor_spinLeft(spinSpeed,speedLeft);
        #if(isWork_OLED == 1)
            OLED_ShowString(1, 9, "BL");
        #endif
    }else if ((irState[0] && irState[1] && irState[2] && !irState[3])
            ||(irState[0] && irState[1] && !irState[2] && !irState[3])
            ||(irState[0] && !irState[1] && !irState[2] && !irState[3]))
    {//1110 or 1100 or 1000
        // 大右转
        Motor_spinRight(speedLeft,spinSpeed);
        #if(isWork_OLED == 1)
            OLED_ShowString(1, 9, "BR");
        #endif
    }else if (!irState[0] && !irState[1] && !irState[2] && !irState[3])
    {//0000 全黑 
        Motor_forward(speedLeft,speedRight);
        #if(isWork_OLED == 1)
            OLED_ShowString(1, 9, " +");
        #endif
    }else if (irState[0] && irState[1] && irState[2] && irState[3])
    {//1111 全空白
        Motor_forward(speedLeft,speedRight);
        // if(count <= 3) {
        //     // 自动后退纠错
        //     Motor_backward(backSpeed,backSpeed);
        // }else{
        //     //到达终点停下
        //     Motor_stop();
        // }
        #if(isWork_OLED == 1)
            OLED_ShowString(1, 9, " B ");
        #endif
    }
}
